
/**
  ******************************************************************************
  * Copyright 2021 The Microbee Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       motor_test.c
  * @author     baiyang
  * @date       2023-2-7
  ******************************************************************************
  */

/*
  mavlink motor test - implements the MAV_CMD_DO_MOTOR_TEST mavlink command so that the GCS/pilot can test an individual motor or flaps
                       to ensure proper wiring, rotation.
 */

/*----------------------------------include-----------------------------------*/
#include "fms.h"

#include <notify/notify.h>
#include <gcs_mavlink/gcs.h>
#include <common/time/gp_time.h>
#include <srv_channel/srv_channel.h>
#include <board_config/board_config.h>
/*-----------------------------------macro------------------------------------*/
// motor test definitions
#define MOTOR_TEST_TIMEOUT_SEC          600     // max timeout is 10 minutes (600 seconds)
/*----------------------------------typedef-----------------------------------*/

/*---------------------------------prototype----------------------------------*/

/*----------------------------------variable----------------------------------*/
static uint32_t motor_test_start_ms;        // system time the motor test began
static uint32_t motor_test_timeout_ms;      // test will timeout this many milliseconds after the motor_test_start_ms
static uint8_t motor_test_seq;              // motor sequence number of motor being tested
static uint8_t motor_test_count;            // number of motors to test
static uint8_t motor_test_throttle_type;    // motor throttle type (0=throttle percentage, 1=PWM, 2=pilot throttle channel pass-through)
static float motor_test_throttle_value;  // throttle to be sent to motor, value depends upon it's type
/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
// motor_test_output - checks for timeout and sends updates to motors objects
void fms_motor_test_output()
{
    // exit immediately if the motor test is not running
    if (!fms.ap.motor_test) {
        return;
    }

    //EXPECT_DELAY_MS(2000);

    // check for test timeout
    uint32_t now = time_millis();
    if ((now - motor_test_start_ms) >= motor_test_timeout_ms) {
        if (motor_test_count > 1) {
            if (now - motor_test_start_ms < motor_test_timeout_ms*1.5) {
                // output zero for 50% of the test time
                mb_motors_output_min(fms.motors);
            } else {
                // move onto next motor
                motor_test_seq++;
                motor_test_count--;
                motor_test_start_ms = now;
                if (!mb_motors_armed(fms.motors)) {
                    mb_motors_armed2(fms.motors, true);
                    brd_set_soft_armed(true);
                }
            }
            return;
        }
        // stop motor test
        fms_motor_test_stop();
    } else {
        int16_t pwm = 0;   // pwm that will be output to the motors

        // calculate pwm based on throttle type
        switch (motor_test_throttle_type) {

            case MOTOR_TEST_COMPASS_CAL:
                //sensor_compass_set_voltage(battery.voltage());
                //sensor_compass_per_motor_calibration_update();
                //FALLTHROUGH;

            case MOTOR_TEST_THROTTLE_PERCENT:
                // sanity check motor_test_throttle value
#if FRAME_CONFIG != HELI_FRAME
                if (motor_test_throttle_value <= 100) {
                    int16_t pwm_min = mb_motors_get_pwm_output_min(fms.motors);
                    int16_t pwm_max = mb_motors_get_pwm_output_max(fms.motors);
                    pwm = (int16_t) (pwm_min + (pwm_max - pwm_min) * motor_test_throttle_value * 1e-2f);
                }
#endif
                break;

            case MOTOR_TEST_THROTTLE_PWM:
                pwm = (int16_t)motor_test_throttle_value;
                break;

            case MOTOR_TEST_THROTTLE_PILOT:
                pwm = rc_get_radio_in(fms.channel_throttle);
                break;

            default:
                fms_motor_test_stop();
                return;
        }

        // sanity check throttle values
        if (pwm >= RC_MIN_LIMIT_PWM && pwm <= RC_MAX_LIMIT_PWM) {
            // turn on motor to specified pwm value
            mb_motors_output_test_seq(fms.motors, motor_test_seq, pwm);
        } else {
            fms_motor_test_stop();
        }
    }
}

// mavlink_motor_test_check - perform checks before motor tests can begin
//  return true if tests can continue, false if not
bool fms_mavlink_motor_control_check(const uint8_t chan, bool check_rc, const char* mode)
{
    // check board has initialised
    if (!fms.ap.initialised) {
        gcs_send_text2(chan, MAV_SEVERITY_CRITICAL,"%s: Board initialising", mode);
        return false;
    }

    // check rc has been calibrated
    if (check_rc && !mb_arming_rc_calibration_checks(true)) {
        gcs_send_text2(chan, MAV_SEVERITY_CRITICAL,"%s: RC not calibrated", mode);
        return false;
    }

    // ensure we are landed
    if (!fms.ap.land_complete) {
        gcs_send_text2(chan, MAV_SEVERITY_CRITICAL,"%s: vehicle not landed", mode);
        return false;
    }

    // check if safety switch has been pushed
    if (brd_safety_switch_state() == SAFETY_DISARMED) {
        gcs_send_text2(chan, MAV_SEVERITY_CRITICAL,"%s: Safety switch", mode);
        return false;
    }

    // check E-Stop is not active
    if (srv_channels_get_emergency_stop()) {
        gcs_send_text2(chan, MAV_SEVERITY_CRITICAL,"%s: Motor Emergency Stopped", mode);
        return false;
    }

    // if we got this far the check was successful and the motor test can continue
    return true;
}

// mavlink_motor_test_start - start motor test - spin a single motor at a specified pwm
//  returns MAV_RESULT_ACCEPTED on success, MAV_RESULT_FAILED on failure
MAV_RESULT fms_mavlink_motor_test_start(const uint8_t chan, uint8_t motor_seq, uint8_t throttle_type, float throttle_value,
                                         float timeout_sec, uint8_t motor_count)
{
    if (motor_count == 0) {
        motor_count = 1;
    }
    // if test has not started try to start it
    if (!fms.ap.motor_test) {
        /* perform checks that it is ok to start test
           The RC calibrated check can be skipped if direct pwm is
           supplied
        */
        if (!fms_mavlink_motor_control_check(chan, throttle_type != 1, "Motor Test")) {
            return MAV_RESULT_FAILED;
        } else {
            // start test
            gcs_send_text(MAV_SEVERITY_INFO, "starting motor test");
            fms.ap.motor_test = true;

            //EXPECT_DELAY_MS(3000);
            // enable and arm motors
            if (!mb_motors_armed(fms.motors)) {
                fms_enable_motor_output();
                mb_motors_armed2(fms.motors, true);
                brd_set_soft_armed(true);
            }

            // disable throttle and gps failsafe
            fms.g.failsafe_throttle = FS_THR_DISABLED;
            fms.g.failsafe_gcs = FS_GCS_DISABLED;
            //fms.g.fs_ekf_action = 0;

            // turn on notify leds
            notify_flags.esc_calibration = true;
        }
    }

    // set timeout
    motor_test_start_ms = time_millis();
    motor_test_timeout_ms = MIN(timeout_sec, MOTOR_TEST_TIMEOUT_SEC) * 1000;

    // store required output
    motor_test_seq = motor_seq;
    motor_test_count = motor_count;
    motor_test_throttle_type = throttle_type;
    motor_test_throttle_value = throttle_value;

    if (motor_test_throttle_type == MOTOR_TEST_COMPASS_CAL) {
        //compass.per_motor_calibration_start();
    }            

    // return success
    return MAV_RESULT_ACCEPTED;
}

// motor_test_stop - stops the motor test
void fms_motor_test_stop()
{
    // exit immediately if the test is not running
    if (!fms.ap.motor_test) {
        return;
    }

    gcs_send_text(MAV_SEVERITY_INFO, "finished motor test");    

    // flag test is complete
    fms.ap.motor_test = false;

    // disarm motors
    mb_motors_armed2(fms.motors, false);
    brd_set_soft_armed(false);

    // reset timeout
    motor_test_start_ms = 0;
    motor_test_timeout_ms = 0;

    // re-enable failsafes
    param_load_variable("VEHICLE", &fms.g.failsafe_throttle);
    param_load_variable("VEHICLE", &fms.g.failsafe_gcs);
    //fms.g.fs_ekf_action.load();

    if (motor_test_throttle_type == MOTOR_TEST_COMPASS_CAL) {
        //compass.per_motor_calibration_end();
    }

    // turn off notify leds
    notify_flags.esc_calibration = false;
}

/*------------------------------------test------------------------------------*/


